// Copyright (c) 2022 ChenJun
// Licensed under the Apache-2.0 License.

#ifndef RM_SERIAL_DRIVER__RM_SERIAL_DRIVER_HPP_
#define RM_SERIAL_DRIVER__RM_SERIAL_DRIVER_HPP_

#include <tf2_ros/transform_broadcaster.h>

#include <geometry_msgs/msg/transform_stamped.hpp>
#include <rclcpp/publisher.hpp>
#include <rclcpp/rclcpp.hpp>
#include <rclcpp/subscription.hpp>
// #include <serial_driver/serial_driver.hpp>
#include <std_msgs/msg/float64.hpp>
#include <std_srvs/srv/trigger.hpp>
#include <visualization_msgs/msg/marker.hpp>

#include "tf2_ros/buffer.h"

#include "tf2_ros/transform_listener.h"
// C++ system
#include <future>
#include <memory>
#include <string>
#include <thread>
#include <vector>
#include <mutex>

#include "auto_aim_interfaces/msg/target.hpp"
#include "rm_serial_driver/SerialEncoder.hpp"

#include "auto_aim_interfaces/msg/dangle_shoot.hpp"
#include "std_msgs/msg/u_int16.hpp"
#include "auto_aim_interfaces/msg/cmd_up_yaw_volt_shoot.hpp"


#define CANUSB_INJECT_SLEEP_GAP_DEFAULT 200 /* ms */
#define CANUSB_TTY_BAUD_RATE_DEFAULT 2000000

namespace rm_serial_driver
{
class RMSerialDriver : public rclcpp::Node
{
public:
  typedef enum {
    CANUSB_SPEED_1000000 = 0x01,
    CANUSB_SPEED_800000  = 0x02,
    CANUSB_SPEED_500000  = 0x03,
    CANUSB_SPEED_400000  = 0x04,
    CANUSB_SPEED_250000  = 0x05,
    CANUSB_SPEED_200000  = 0x06,
    CANUSB_SPEED_125000  = 0x07,
    CANUSB_SPEED_100000  = 0x08,
    CANUSB_SPEED_50000   = 0x09,
    CANUSB_SPEED_20000   = 0x0a,
    CANUSB_SPEED_10000   = 0x0b,
    CANUSB_SPEED_5000    = 0x0c,
  } CANUSB_SPEED;
  
  typedef enum {
    CANUSB_MODE_NORMAL          = 0x00,
    CANUSB_MODE_LOOPBACK        = 0x01,
    CANUSB_MODE_SILENT          = 0x02,
    CANUSB_MODE_LOOPBACK_SILENT = 0x03,
  } CANUSB_MODE;

  typedef enum {
    CANUSB_FRAME_STANDARD = 0x01,
    CANUSB_FRAME_EXTENDED = 0x02,
  } CANUSB_FRAME;

  typedef enum {
      CANUSB_INJECT_PAYLOAD_MODE_RANDOM      = 0,
      CANUSB_INJECT_PAYLOAD_MODE_INCREMENTAL = 1,
      CANUSB_INJECT_PAYLOAD_MODE_FIXED       = 2,
  } CANUSB_PAYLOAD_MODE;

  // copy from diankong
  typedef enum
  {
      CAN_FROM_NUC=0x005, //can1 NUC数据

      CAN_UP_GIMBAL_SLAVE_FREE=0x007, //can1 NUC数据

      CAN_M_CAN1_ID=0x1FF,
      CAN_SHOOT_UP_TRIGGER=0x207,

      CAN_G_STM_CAN1_ID = 0x2FF,  
      CAN_YAW_DOWN = 0x209, 

      CAN_G_CAN2_ID=0x2FF,
      CAN_YAW_UP = 0x209,
      CAN_PITCH_DOWN = 0x20A,
      CAN_PITCH_UP = 0x20B,

      CAN_M_UP_CAN2_ID = 0x200, 
      CAN_SHOOT_UP_FRIC_L = 0x202,
      CAN_SHOOT_UP_FRIC_R = 0x203,
      CAN_SHOOT_DOWN_TRIGGER = 0x204,

      CAN_M_DOWN_CAN2_ID = 0x1FF, 
      CAN_SHOOT_DOWN_FRIC_L = 0x207,
      CAN_SHOOT_DOWN_FRIC_R = 0x208,
      
  } can_msg_id_e;


  explicit RMSerialDriver(const rclcpp::NodeOptions & options);

  ~RMSerialDriver() override;

private:
  void getParams();

  void receiveData();

  void sendData(auto_aim_interfaces::msg::DangleShoot::SharedPtr msg);

  void reopenPort();

  void setParam(const rclcpp::Parameter & param);

  void resetTracker();

  // void 
  // void predict_and_gen_ctr(const auto_aim_interfaces::msg::Target::SharedPtr &msg, ctr_data_raw_float & ctr, float pipeline_process_latency_ms);
  int adapter_init(const char *tty_device, int baudrate);
  int command_settings(int tty_fd, CANUSB_SPEED speed, CANUSB_MODE mode, CANUSB_FRAME frame);
  int send_data_frame(int tty_fd, CANUSB_FRAME frame, unsigned char id_lsb, unsigned char id_msb, unsigned char data[], int data_length_code);
  int generate_checksum(const unsigned char *data, int data_len);
  int frame_recv(int tty_fd, unsigned char *frame, int frame_len_max);
  int frame_send(int tty_fd, const unsigned char *frame, int frame_len);
  int frame_is_complete(const unsigned char *frame, int frame_len);
  void ug_ctr_call_back(const auto_aim_interfaces::msg::CmdUpYawVoltShoot::SharedPtr msg);
  void sendOneCtrData(ctr_data_raw_float & myctr);
  void timerCallback();
  // Serial port
  // std::unique_ptr<IoContext> owned_ctx_;
  std::string device_name_;
  // std::unique_ptr<drivers::serial_driver::SerialPortConfig> device_config_;
  // std::unique_ptr<drivers::serial_driver::SerialDriver> serial_driver_;
  int usb_can_tty_fd = -1;
  std::mutex usb_can_write_mutex; 

  // compensation coefficients
  float earth_comp_k_;
  float earth_comp_b_;
  float static_error_electric_mechanics_pitch_;
  float static_error_electric_mechanics_yaw_;
  float shoot_action_delay_;
  float shoot_speed_;
  float critical_omega_;
  float shoot_gap_;

  // Param client to set detect_colr
  using ResultFuturePtr = std::shared_future<std::vector<rcl_interfaces::msg::SetParametersResult>>;
  bool initial_set_param_ = false;
  uint8_t previous_receive_color_ = 0;
  rclcpp::AsyncParametersClient::SharedPtr detector_param_client_;
  ResultFuturePtr set_param_future_;


  std::shared_ptr<tf2_ros::TransformListener> tf_listener_{nullptr};
  std::unique_ptr<tf2_ros::Buffer> tf_buffer_;

  // Service client to reset tracker
  rclcpp::Client<std_srvs::srv::Trigger>::SharedPtr reset_tracker_client_;

  // Aimimg point receiving from serial port for visualization
  visualization_msgs::msg::Marker aiming_point_;

  // Broadcast tf from odom to gimbal_link
  double timestamp_offset_ = 0;
  std::unique_ptr<tf2_ros::TransformBroadcaster> tf_broadcaster_;

  rclcpp::Subscription<auto_aim_interfaces::msg::DangleShoot>::SharedPtr target_sub_;

  // For debug usage
  rclcpp::Publisher<std_msgs::msg::Float64>::SharedPtr latency_pub_;
  rclcpp::Publisher<visualization_msgs::msg::Marker>::SharedPtr marker_pub_;

  std::thread receive_thread_;
  std::string direct_dangle_shoot_topic_;

  // rclcpp::Publisher<auto_aim_interfaces::msg::DangleShoot>::SharedPtr float_ctr_publisher_;
  rclcpp::Publisher<std_msgs::msg::UInt16>::SharedPtr ecd_yaw_publisher_;

  rclcpp::Subscription<auto_aim_interfaces::msg::CmdUpYawVoltShoot>::SharedPtr ug_ctr_subscription_;

  rclcpp::TimerBase::SharedPtr timer_;
  // rclcpp::Time last_aim_time_;
  std::chrono::steady_clock::time_point last_aim_info_time_;
  float chassis_yaw_spin_ = 1.0;

};
}  // namespace rm_serial_driver

#endif  // RM_SERIAL_DRIVER__RM_SERIAL_DRIVER_HPP_
